
#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/features/boundary.h>
#include <pcl/features/eigen.h>
#include <pcl/features/feature.h>
#include <pcl/features/normal_3d.h>
#include <pcl/impl/point_types.hpp>//
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include<pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/ModelCoefficients.h>

#include<pcl/console/parse.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_circle3d.h>
#include <pcl/sample_consensus/sac_model_circle.h>
#include <vector>

#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
//帮助函数
void ShowHelp(const char * program_name);

 //预处理 下采样滤波
int Pretreatment(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_o, const float &leaf_size, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_);
//直通滤波
int Passfilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string a, const float min, const float max, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_);
//半径滤波
int RadiusFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, double R, int n, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
//统计滤波
int  Statsticalfilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_);
//法向量
int NormalMath( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr normals);
//分割
int Segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr normals, const int &model,
pcl::PointIndices::Ptr inliers_, pcl::ModelCoefficients::Ptr cofficients_, const int &Iterations, const double &distance);
//提取分割点集
int Extractcloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointIndices::Ptr inliers_, bool Negative_Extra, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inliers);
//Kdtree
int Kdtreecloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const int min, const int max, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_);


//旋转
int Transformx(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out,float theta);
int Transformy(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out,float theta);
//分割多个平面   ---- vector 
int SegmentModel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, 
std::vector<pcl::ModelCoefficients::Ptr>& seg_coefficients, int faces, int model, float R, bool last);
// 拟合 3D 圆
int ConsensusCircle3D(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<int>& inliersCircle3D);
//拟合 2D 圆
int ConsensusCircle2D(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<int> &inliersCircle2D);
//可视化  两个点云作对比
boost::shared_ptr<pcl::visualization::PCLVisualizer>
SimpleVIS( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
//可视化点云集显示
void Visual_clouds(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds);
//保存点云
int Save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>clouds);

//提取边界
int Boundary_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
//单个显示
void Visual_cloudone(pcl::PointCloud<pcl::PointXYZ>::Ptr clouds);
//欧式距离提取
int EuclideCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out,
 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Eucluextra);

// 线距离 
int Distance_line(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds_line,  std::vector<float> distance);
//平面 组
std::vector<std::pair<int, int >>  Group_plane(std::vector<pcl::ModelCoefficients::Ptr>& seg_coefficients);

//计算 两个平面夹角
float Angle_plane(pcl::ModelCoefficients::Ptr a, pcl::ModelCoefficients::Ptr b);
//计算面 距离
std::vector<float> Distance_plane(Eigen::Vector4f &centroid, std::vector<pcl::ModelCoefficients::Ptr> &seg_coefficients, std::vector<std::pair<int, int >> groups);
//点到平面距离
float Dis_dp(Eigen::Vector4f &dot, pcl::ModelCoefficients::Ptr plane);
//求直线到面交点    短线生成的面 垂直于长线 的交点
int Line_Plane_point(std::vector<pcl::ModelCoefficients::Ptr> seg_coefficients_line, std::vector<pcl::PointXYZ> point_cross );